clear all;
clc;close all;
load('widebeamfactor.mat')      % 宽波束
Nt=16;Nr=16;
Nsc=4096;
RBNum=264;
Ncp=288;
Nsym=40;
N_ISAC=14; % 每这么多个符号测量一次
fc=6.9e9;c=3e8;
lambda=c/fc;d=lambda/2;% 天线间距
ScS=120e3;fs=ScS*Nsc;  % 采样率为带宽
SlotNum_zp=64; fNum_zp=4096;
TPRI=N_ISAC/ScS;
delta_T=TPRI*Nsym;  % 测量时间间隔

n_angle=360;        % 角度扫描分辨率
SNR=10;
    % 最大速度
    Velo_Max=c/(4*TPRI*fc);
    % OFDM速度分辨率
    Velo_Resolu=c*Nsym/SlotNum_zp/(2*TPRI*Nsym*fc);
    % OFDM最大探测距离
    Dist_Max=Nsc*c/(2*Nsc*ScS);
    % OFDM距离精度
    Dist_Accur=c/(2*Nsc*ScS);
    disp(['最大探测速度: ',num2str(Velo_Max),' 速度分辨率: ',num2str(Velo_Resolu),...
        ' 最大探测距离 ',num2str(Dist_Max),' 距离分辨率: ',num2str(Dist_Accur),...
        ' 角度分辨率: ',num2str(180/n_angle)]);
% 车辆信息初始化
velo=20;
UAV.v=velo;   % Unmanned Aerial Vehicle
UAV.x=-50;
UAV.y=50;
UAV.state=[UAV.x;UAV.y;UAV.v;];    % 坐标
UAV.acc=[atand(UAV.x/UAV.y);sqrt(UAV.state(1)^2+UAV.state(2)^2);UAV.state(3)];               % 近似量初始化
UAV.appro=UAV.acc;  % 实际的角度/距离/速度
UAV.KF=UAV.state;
UAV.EKF=UAV.state;
A=[1,0,delta_T;0,1,0;0,0,1];        % 系统状态更新矩阵
% 系统状态矩阵 x1--theta;x2--d;x3--v;f1--theta状态转移矩阵；f2--d状态转移矩阵；f3--v状态转移矩阵
% syms x1 x2 x3
% f1=x1+asin(x3*delta_T*sind(x1)/x2);
% f2=x2-x3*delta_T*cosd(x1);
% f3=x3;
% f={f1,f2,f3};
% J=jacobian([f1;f2;f3],[x1,x2,x3]);
% G=matlabFunction(J,'Vars',{x1,x2,x3});     % 状态转移函数的雅克比矩阵
H=[1,0,0;0,1,0;0,0,1];                     % 观测矩阵
Q=0;                                       % 状态转移噪音的协方差矩阵
R1=diag([Dist_Accur^2/8*(1+cos(180/n_angle)),Dist_Accur^2/8*(1+cos(180/n_angle)),Velo_Resolu^2]);        % 观测噪音协方差矩阵-----应改为dcosθ的方差但是我算不来                                       % 测量噪音的协方差矩阵
P1=0.01*eye(3);                             % 估计均方误差矩阵初值
P2=0.01*eye(3);                             % 估计均方误差矩阵初值
%R2=diag([(180/n_angle)^2/4, Dist_Accur^2/4, Velo_Resolu^2/4]);        % 观测噪音协方差矩阵 theta;d;v

storage_state = [];       % 存储真实运动的状态变量值，x;y;v ，后期用于对比
storage_appro = [];       % 运动状态近似值         theta;d;v
storage_acc = [];         % 运动状态真值
storage_mea = [];         % 存储测量值             x,y,v
storage_KF = [];          % KF值
storage_EKF = [];         % EKF值
storage_Num=10;  % 测量次数

% TDM
    csi_seq=CSI_RS_Seq(RBNum*12,8,4); % 通感符号放在第四个符号
    % if mod(i_sym,Nt) ~= 0
    %     tx = mod(i_sym,Nt);
    % else 
    %     tx = 8;
    % end
    TX_FD(1:RBNum*6)=csi_seq(1:RBNum*6);
    TX_FD(Nsc-RBNum*6+1:Nsc)=csi_seq(1+RBNum*6:end);
%% IFFT调制+CP
    Data_TD=ifft(TX_FD,Nsc);
    TX_TD=zeros(Nsc+Ncp,1);
    TX_TD(1:Ncp)=Data_TD(Nsc-Ncp+1:Nsc);
    TX_TD(Ncp+1:Ncp+Nsc)=Data_TD;
    TX_TD=BeamH*TX_TD.';
    TX=zeros(Nt,Nsc+Ncp,Nsym);
    for i_sym=1:Nsym
        TX(:,:,i_sym)=TX_TD;
    end
    TX=reshape(TX,Nt,[]);

%% 信道 假设其他径不变，只变目标径
L = 3;% 多径数
theta_t = randi([-180,180],L-1,1);
theta_r = randi([-180,180],L-1,1);
% 计算阵列方向向量
At = zeros(Nt, L-1);
Ar = zeros(Nr, L-1);
for l = 1:L-1
    At(:, l) = exp(-1j * (0:Nt-1)' * pi * sind(theta_t(l))) / sqrt(Nt);
    Ar(:, l) = exp(-1j * (0:Nr-1)' * pi * sind(theta_r(l))) / sqrt(Nr);
end
alpha=(randn(1, L-1) + 1j * randn(1, L-1)) / sqrt(2); % 瑞利衰落
H_tmp = sqrt(1/(L-1)) * Ar * diag(alpha) * At';
RX_tmp = H_tmp*TX;

% 目标径
for i_sto=1:storage_Num
    % 更新参数
    storage_state = [storage_state, UAV.state]; % 记录实际航迹
    storage_acc = [storage_acc, UAV.acc];       % 记录实际的参数
    storage_KF = [storage_KF, UAV.KF];
    %storage_EKF = [storage_EKF, UAV.EKF];
    theta_target=UAV.acc(1);                   % 角度
    dis_target=UAV.acc(2);                      % 距离
    velo_target=UAV.acc(3)*cosd(UAV.acc(1));      % 速度
    fd=2*velo_target*fc/c;

        t=[(Nsc+Ncp)*Nsym*(i_sto-1):1:(Nsc+Ncp)*Nsym*i_sto-1]/fs*N_ISAC;
        tau=2*(UAV.acc(2))/c;
        SampleOffset=floor(tau*fs);
    H_target = zeros(Nt,Nr,length(t));
    % k = -Nsc/2 : Nsc/2-1;
    % f_k = fc+ScS*k;
    %     delay=exp(-1j*2*pi*f_k(ns)*tau);
    for i_t=1:length(t)
        doppler = exp(1j * 2 * pi * fd * t(i_t));
        At = exp(-1j * (0:Nt-1)' * pi * sind(theta_target)) / sqrt(Nt);
        Ar = exp(-1j * (0:Nr-1)' * pi * sind(theta_target)) / sqrt(Nr);
        H_target(:,:,i_t) = Ar * doppler * At';
    end
    for i_t=1:length(t)
        RX_target(:,i_t)=H_target(:,:,i_t)*TX(:,i_t);
    end
    %RX_target=awgn(RX_target,SNR);
    for ant=1:Nr
        tmp=[zeros(1,SampleOffset),squeeze(RX_target(ant,:))];
        RX_target(ant,:)=tmp(1:length(RX_target));
    end
    
    RX=RX_target+RX_tmp;   
    RX=awgn(RX,SNR,'measured');
    %RX=1/Nr*BeamH'*RX;
    RX_FD_tmp=zeros(Nsc+Ncp,Nsym,Nr);   %存储接收数据
    m=1;
    for i_sym=1:Nsym
        for i_nsc = 1:Nsc+Ncp
            RX_FD_tmp(i_nsc,i_sym,:)=RX(:,m);    
            m=m+1;
        end
    end
    RX_FD_tmp = RX_FD_tmp(Ncp+1:Ncp+Nsc,:,:);     % 去CP&维度[Nsc Nsym Nr]
    % 变换到频域
    RX_FD = zeros(Nsc,Nsym,Nr);
    for nr = 1:Nr
        for i = 1:Nsym
            RX_FD(:,i,nr) = fft(RX_FD_tmp(:,i,nr),Nsc);
        end
    end
    %RX_FD=1/Nr*BeamH'*RX_FD;
    H_est=zeros(Nsc,Nsym,Nr);
    for i=1:Nsym
        for j=1:Nr
            H_est(:,i,j)=RX_FD(:,i,j)./TX_FD.';
        end
    end
    H_est(RBNum*6+1:Nsc-RBNum*6,:,:)=0;
    % figure(1);
    % stem(unwrap(angle(squeeze(H_est(1,1,:)))))
    % figure(2);
    % stem(unwrap(angle(squeeze(H_est(:,1,1)))))
    % figure(3);
    % stem(unwrap(angle(squeeze(H_est(1,:,1)))))
    
    %[theta_est] = DoA(H_est,ScS,fc,0.5,1,0)
    [SpeedDistMatrix,d,v] = tdfft(H_est(:,:,2),  SlotNum_zp, fNum_zp, 1,ScS,14/ScS,fc,1,Ncp,Nsym);
    %% CFAR---求信噪比
    N_tc=4;N_gc=4;N_tr=4;N_gr=4;CFAR_th=16;% CFAR门限/dB
    Power_sum=abs(SpeedDistMatrix).^2;
    % % 先取第一根天线
    % H_tmp=H_est(:,:,1);
    % H_fft2=fft2(H_tmp,fNum_zp,SlotNum_zp);
    % Power_sum=10*log10(abs(H_fft2).^2);
    % % 合并天线
    % for ant=1:Nr
    %     Power_sum=Power_sum+abs(H_est(:,:,ant)).^2;
    % end
    % Power_sum=Power_sum/Nr;
    JudgeMatrix=zeros(fNum_zp+2*(N_tc+N_gc),SlotNum_zp+2*(N_gr+N_tr));
    JudgeMatrix(N_tc+N_gc+1:fNum_zp+N_tc+N_gc,N_gr+N_tr+1:SlotNum_zp+N_gr+N_tr)=Power_sum;
    % 
    SINR_CFAR=zeros(fNum_zp,SlotNum_zp);
    CFAR_Output=zeros(fNum_zp,SlotNum_zp);
    Ref_num =(2*(N_tc+N_gc)+1)*(2*(N_tr+N_gr)+1)-(2*N_gc+1)*(2*N_gr+1);%参考单元个数
    for i = N_tc+N_gc+1:fNum_zp+N_tc+N_gc
      for j= N_tr+N_gr+1:N_tr+N_gr+SlotNum_zp
            rcMatrix = JudgeMatrix(i-(N_tc+N_gc):i+(N_tc+N_gc),j-(N_tr+N_gr):j+(N_tr+N_gr));% 参考单元矩阵
            %去除保护单元
            rcMatrix(N_tc+1:N_tc+1+2*N_gc,N_tr+1:N_tr+1+2*N_gr) = 0;
            %求参考单元均值
            P_noise = sum(sum(rcMatrix))/Ref_num;
            SINR_CFAR(i-(N_tc+N_gc),j-(N_tr+N_gr)) = 10*log10(JudgeMatrix(i,j)/P_noise);
    %         if SINR_CFAR(i-(N_tc+N_gc),j-(N_tr+N_gr)) >= CFAR_TH
    %              CFAR_Output(i-(N_tc+N_gc),j-(N_tr+N_gr))= 1;
    %         else
    %              CFAR_Output(i-(N_tc+N_gc),j-(N_tr+N_gr))= 0;
    %         end
       end
    end
    % figure(1);
    % mesh(abs(SpeedDistMatrix));
    % figure(2);
    % mesh(abs(SINR_CFAR));
    [~,index]=max(SINR_CFAR(:));
    [MaxIndexDist, MaxIndexVel] = ind2sub(size(SINR_CFAR), index);
        Dist_estim=(MaxIndexDist-1)*c/(2*ScS*fNum_zp);
        Velo_estim=(MaxIndexVel-SlotNum_zp/2-1)*Nsym/SlotNum_zp*c/(2*TPRI*Nsym*fc);              
    H_angle=squeeze(H_est(MaxIndexDist,MaxIndexVel,:));
        % 扫描到达角
        Rd = (H_angle*H_angle');          
        theta_grid = [-90:180/n_angle:90];
        P_Scan = zeros(length(theta_grid),1);
        for i_theta = 1:length(theta_grid)
            theta = theta_grid(i_theta);
            a_x = exp(-1j*pi*(0:Nr-1)'*sind(theta));
            P_Scan(i_theta) = abs(a_x'*Rd*a_x);
        end
        [~,idx] = max(P_Scan);
        theta_est = theta_grid(idx);
        % 转换为与x轴正向的夹角
        if theta_est<0
            theta_est=abs(theta_est)+90;
        else
            theta_est=90-theta_est;
        end
        figure(3);
        plot(theta_grid,P_Scan);
        title('均匀线阵 (ULA) 波束扫描 DOA 估计');
        xlabel('角度 (°)');
        ylabel('波束扫描功率');
        UAV.mea=[Dist_estim*cosd(theta_est);Dist_estim*sind(theta_est);Velo_estim/cosd(180-theta_est)];              %%%% 是存Velo_estim还是Velo_estim/cosd(theta_est)？
        storage_mea = [storage_mea, UAV.mea];       % 记录实际的参数
 %% KF && EKF
        [UAV.KF, P1] = KF(P1, A, H, Q, R1, UAV.KF, UAV.mea);
        %[UAV.EKF, P2] = KF(P2, G, H, Q, R2, UAV.KF, UAV.mea);
 
    disp(['-----------------------第 ',num2str(i_sto),' 次测量------------------------']);
    disp(['距离测量值  ',num2str(Dist_estim),...
    '    多普勒速度测量值 ',num2str(Velo_estim), ...
    '    角度测量值 ',num2str(theta_est)]);
    disp(['距离真值    ',num2str(UAV.acc(2)),...
    '    多普勒速度真值 ',num2str(UAV.acc(3)*cosd(UAV.acc(1))), ...
    '     角度真值 ',num2str(UAV.acc(1)+180)]);
    disp(['x真值为    ',num2str(UAV.state(1)),...
        '        y真值为    ',num2str(UAV.state(2)),...
        '      v真值为    ',num2str(UAV.state(3))]);
    disp(['x测量值    ',num2str(Dist_estim*cosd(theta_est)),...
        '   y测量值    ',num2str(Dist_estim*sind(theta_est)), ...
        ' v测量值    ',num2str(Velo_estim/cosd(180-theta_est))]);
    disp(['x滤波值    ',num2str(UAV.KF(1)),...
    '   y滤波值    ',num2str(UAV.KF(2)), ...
    '   v滤波值    ',num2str(UAV.KF(3))]);
    disp('-----------------------------------------------------------');
%% 更新状态
    UAV.state=A*UAV.state;
%     d_acc=-1*UAV.appro(3)*delta_T*cosd(abs(UAV.appro(1)));
%     angle_appro=asin(sind(abs(UAV.appro(1)))*UAV.appro(3)*delta_T/UAV.appro(2));
%     UAV.appro=UAV.appro+[angle_appro;d_appro;0];   % 近似状态演化方程
    UAV.acc=[atand(UAV.state(1)/UAV.state(2));sqrt(UAV.state(1)^2+UAV.state(2)^2);UAV.state(3)];
end
drawpic(storage_Num,storage_state,storage_mea,storage_KF);